#include <vision/CrateTracker.h>

#include <huniplacer_3d/Render.h>
#include <huniplacer_3d/huniplacer_3d.h>
#include <huniplacer_3d/Callbacks.h>
#include <huniplacer_3d/main.h>

#include <huniplacer_3d/viewCheckSrv.h>

#include <iostream>
#include <sstream>

#include <stdio.h>

std::map<std::string, CrateModel*> crates;

/**
 * Function to act on received messages containing motor positions
 *
 */
void moveCallback(const deltarobotnode::motions::ConstPtr& msg) {
	getHuniplacerData()->modeldata.robot->setPosition(msg->x[0], msg->y[0], msg->z[0]);
		
	point3 point3(msg->x[0], msg->y[0], msg->z[0]);

	motionf motion;
	getHuniplacerData()->modeldata.ikmodel->point_to_motion(point3, motion);
	
	getHuniplacerData()->paneldata.motor_angles[0]->current_value = motion.angles[0];
	getHuniplacerData()->paneldata.motor_angles[1]->current_value = motion.angles[1];
	getHuniplacerData()->paneldata.motor_angles[2]->current_value = motion.angles[2];
	
	std::ostringstream logData;
	logData << "Move to: x=" << msg->x[0] << " y=" << msg->y[0] << " z=" << msg->z[0];
	getHuniplacerData()->paneldata.log->write(logData.str());
	
	//while(1) {}
	ROS_INFO("Move To: x=%f y=%f z=%f", msg->x[0], msg->y[0], msg->z[0]);
}

/**
 * Function to act on received Crate Event messages
 *
 */
void crateCallback(const vision::CrateEventMsg::ConstPtr& msg) {
	ROS_INFO("MSG RECV: %i %s", msg->event, msg->crate.name.c_str());
	CrateModel* this_crate =
			crates.find(msg->crate.name) != crates.end() ?
					crates.find(msg->crate.name)->second : NULL;
	
	if (this_crate == NULL) {
		getHuniplacerData()->paneldata.log->write("Crate " + msg->crate.name + " not present. Creating new one");
		
		ROS_INFO("Crate %s not present. Creating new one", msg->crate.name.c_str());
		this_crate = new CrateModel(getHuniplacerData()->modeldata.robot, msg->crate.name, point3(50, 10, 50));
		crates.insert(
				std::pair<std::string, CrateModel*>(msg->crate.name,
						this_crate));
		point3 p(msg->crate.x, GROUND_PANE, msg->crate.y);
		this_crate->setLocation(p);
		this_crate->setRotation(msg->crate.angle);
		this_crate->setAlpha(1.0);
	}
	
	switch (msg->event) {
	case CrateEvent::type_in:
		//Already done above
		break;
	case CrateEvent::type_out:
		getHuniplacerData()->paneldata.log->write("Crate " + msg->crate.name + " deleted");
		
		ROS_INFO("Crate %s deleted", msg->crate.name.c_str());
		crates.erase(msg->crate.name);
		delete this_crate;
		break;
	case CrateEvent::type_moving: {
		getHuniplacerData()->paneldata.log->write("Crate " + msg->crate.name + " moving");
		
		ROS_INFO("Crate %s moving", msg->crate.name.c_str());
		point3 p(msg->crate.x, GROUND_PANE, msg->crate.y);
		this_crate->setAlpha(0.3);
	}
		break;
	case CrateEvent::type_moved: {
		getHuniplacerData()->paneldata.log->write("Crate " + msg->crate.name + " stable");
		
		ROS_INFO("Crate %s s", msg->crate.name.c_str());
		point3 p(msg->crate.x, GROUND_PANE, msg->crate.y);
		this_crate->setLocation(p);
		this_crate->setRotation(msg->crate.angle);
		this_crate->setAlpha(1);
	}
		break;
	}
}

/*
 * This function makes sure there are no objects blocking the view to crate <req.name> of the top camera
 */
bool viewCheckCallback(huniplacer_3d::viewCheckSrv::Request &req, huniplacer_3d::viewCheckSrv::Response &res) {
	getHuniplacerData()->paneldata.log->write("checking view in callback!");
	for (int next_corner = 0; next_corner < 6; next_corner++) {
		//Move 'virtual' camera of OpenGL to the same position of the actual camera
		float copy_xRotation = getXRotation();
		float copy_yRotation = getYRotation();
		
		setRotation(-90, 0);
		
		float copy_xMovement = getXMovement();
		float copy_yMovement = getYMovement();
		float copy_zMovement = getZMovement();
		
		//This probably isn't a very accurate location of the top camera
		setMovement(0, 200, -100);
		
		//Only draw the robot and the crates
		picking_render(true, true, false, false, false, false, false);
		setRotation(copy_xRotation, copy_yRotation);
		setMovement(copy_xMovement, copy_yMovement, copy_zMovement);
		
		std::string name_target = req.name;

		CrateModel* scan_target = getHuniplacerData()->modeldata.robot->getCrate(name_target);
		if (scan_target == 0) {
			getHuniplacerData()->paneldata.log->write("ERROR: viewCheck was unable to find scan target: " + name_target);
			return false;
		}

		float precision = 2.f; //1.0 == 1mm... TODO: do this with a calculation (find out how many .f per pixel at the depth of the crate)
		
		int blocked_pixels = 0;
						
		for (float x = -scan_target->getSize().x/2 + precision; x < scan_target->getSize().x/2 - precision; x+= precision) {
			for (float z = -scan_target->getSize().z/2 + precision; z < scan_target->getSize().z/2 - precision; z+= precision) {
				GLdouble pixel_x = -1;
				GLdouble pixel_y = -1;
				GLdouble pixel_z = -1; //This value always ends up as 1, the screen has no depth. Pay no attention to this value here & below
	
				GLint project_result  = gluProject(	x, 
																scan_target->getSize().y/2, 
																z, 
																scan_target->modelview, 
																scan_target->projection, 
																scan_target->viewport, 
																&pixel_x, 
																&pixel_y, 
																&pixel_z);
																				
				if (project_result != GL_TRUE){
					std::cout << "project_result != GL_TRUE" << std::endl;
				} else {
					unsigned char pixel_color[3];
					glReadPixels(pixel_x, pixel_y, 1, 1, GL_RGBA, GL_UNSIGNED_BYTE, pixel_color);
					//std::cout << "pixel_color: " << (int)pixel_color[0] << ":" << (int)pixel_color[1] << ":" << (int)pixel_color[2] << std::endl;
	
					if ((pixel_color[0] == 255 && pixel_color[1] == 0 && pixel_color[2] == 0) ||
						(pixel_color[0] == 0 && pixel_color[1] == 255 && pixel_color[2] == 0)) {
			
					} else {
						if( !scan_target->compare_id( pixel_color ) ){
							blocked_pixels++;
						}
					}
				}
			}
		}

		std::cout << "Blocked pixels: " << blocked_pixels << std::endl;

		if (blocked_pixels > 0){
			deltarobotnode::motionSrv robotMotion;

			//TODO: insert the 6 corner positions of the movement cloud (with y halfway the movement cloud)
			switch (next_corner){
			case 0 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			case 1 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			case 2 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			case 3 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			case 4 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			case 5 :
				getHuniplacerData()->modeldata.robot->setPosition(0, 0, 0);
				break;
			}
			
			if (!getHuniplacerData()->modeldata.robot->move_actual_robot()) {
				getHuniplacerData()->paneldata.log->write("ERROR: viewCheck was unable to move the robot");
				return false;	
			}
		} else {
			return true;
		}
	}
	return false;
}
		


/**
 * Function to act on received demo progress messages
 *
 */
void demoProgressCallback(const huniplacer_3d::demoProgressMsg::ConstPtr& msg) {
	getHuniplacerData()->paneldata.progressBar->current_value = msg->currentStep;
	getHuniplacerData()->paneldata.progressBar->max_value = msg->totalSteps;
	
	if (msg->currentStep == msg->totalSteps) {
		getHuniplacerData()->paneldata.i_panel->remove_part(getHuniplacerData()->paneldata.progressBar);
	}
	
	std::ostringstream logData;
	logData << "Demo progress: " << (int)msg->currentStep << "/" << (int)msg->totalSteps;
	getHuniplacerData()->paneldata.log->write(logData.str());
}

/**
 * Function to act on received delta robot position messages
 *
 */
void positionCallback(const deltarobotnode::motions::ConstPtr& msg){
	getHuniplacerData()->modeldata.robot->setNewAnimation(msg.get()->x, msg.get()->y, msg.get()->z,msg.get()->speed);
}

/**
 * Function to act on top view messages
 *
 */
void topviewCallback(const huniplacer_3d::TopViewMsg::ConstPtr& msg){
	CrateModel* this_crate =
				crates.find(msg->name) != crates.end() ?
						crates.find(msg->name)->second : NULL;

	getHuniplacerData()->paneldata.log->write("topviewCallback: fresh crate content data! :D");

	if(this_crate != NULL){
		for(int i1 = 0; i1 < msg->rows; i1++){
			for(int i2 = 0; i2 < msg->cols; i2++){
				this_crate->set_content_color(i1 + i2*msg->cols, 	1.0f/255*msg->red[i1 + i2*msg->cols], 
																					1.0f/255*msg->green[i1 + i2*msg->cols], 
																					1.0f/255*msg->blue[i1 + i2*msg->cols]);
																					
				//this_crate->beat_detected[i1 + i2*msg->cols] = (float)msg->data[i1 + i2*msg->cols];	//TODO: 	this is probable in a wrong scale, I'm assuming that values of
																																//TODO:	beats_detected are in the range of 0 to 255. The values of
				 																												//TODO:	this_crate->beat_detected must be in the range of 0.0f to 1.0f
				
				std::cout << "data: posx: " << i2 << ", posy: " << i1 << ", data: " << msg->data[i1 + i2*msg->cols] << std::endl;
			}
		}
	} else {
		getHuniplacerData()->paneldata.log->write("Unknown crate: " + msg->name);
	}
}
